#include <stdlib.h>
#include <string.h>
#include <Wirish_math.h >
#include "Wirish.h"
#define  FlightAngle_ARG_MODE
#define ROLL 0
#define PITCH 1
#define YAW 2
#define THROTTLE 3
#define MODE 4
#define AUX 5
#define AUX2 6
#define AUX3 7
#define XAXIS 0
#define YAXIS 1
#define ZAXIS 2
#define LASTAXIS 3
#define LEVELROLL 3
#define LEVELPITCH 4
#define LASTLEVELAXIS 5
#define HEADING 5
#define LEVELGYROROLL 6
#define LEVELGYROPITCH 7
#define ALTITUDE 8
#define ZDAMPENING 9
float G_Dt = 0.002;
unsigned long previousTime = 0;
unsigned long currentTime = 0;
unsigned long deltaTime = 0;
// sub loop times
//unsigned long oneHZpreviousTime;
unsigned long tenHZpreviousTime;
unsigned long twentyFiveHZpreviousTime;
unsigned long fiftyHZpreviousTime;
unsigned long hundredHZpreviousTime;
unsigned long fastTelemetryTime = 0;
unsigned long frameCounter = 0; // main loop executive frame counter

// Global variables
//int32 gyroADC[3];
float YawGyroValue   = 0;
float PitchGyroValue = 0;
float RollGyroValue  = 0;
int32 YawGyroZero    = 0;
int32 PitchGyroZero  = 0;
int32 RollGyroZero   = 0;
int xPin   = 15; // PC0, the input pin for the x
int yPin   = 16; // PC1 the input pin for the x
int zPin   = 17; // PC2, the input pin for the x

 
float xValue = 0;  // value coming from the sensor
float yValue = 0;  // value coming from the sensor
float zValue = 0;  // value coming from the sensor

int32 xValueZero = 0;   
int32 yValueZero = 0;   
int32 zValueZero = 0;   

float accVector[3];//加速度各轴投影分量
float accXZAngle;//x对Z的角度
float accYZAngle;//  y对Z的角度
 int32  gyroADC[3],accADC[3],magADC[3];
 int16  accSmooth[3]; 
 int16  acc_1G;             // this is the 1G measured acceleration
 int16 gyroData[3] = {0,0,0};
 int16 gyroZero[3] = {0,0,0};
 int16 angle[2]    = {0,0};  // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
static float    BaroAlt = 0.0f;
static float    EstVelocity = 0.0f;
static float    EstAlt = 0.0f;

boolean firstSample = true;
int lastTime = 0;
int interval = 0;
float wGyro = 10.0;

float RwAcc[3];    //projection of normalized gravitation force vector on x/y/z axis, as measured by accelerometer
float Gyro_ds[3];  //Gyro readings, in degrees per second scale
float RwGyro[3];   //Rw obtained from last estimated value and gyro movement
float Awz[2];      //angles between projection of R on XZ/YZ plane and Z axis (deg)
float RwEst[3];    //pitch roll and yaw attitude fused sensor values
float RwEstSmoothPitch = 0;
float RwEstSmoothRoll = 0;
float gyroRollAngle;
float gyroPitchAngle;
#define PIDLEVEL   5
#define PIDMAG     6
 uint8 P8[7], I8[7]; //8 bits is much faster and the code is much shorter
 uint8 rcRate8;
 uint8 rcExpo8;
 uint8 rollPitchRate;
 uint8 yawRate;
 uint8 dynThrPID;
//-----------------------------RC------------------------------------------------------------------------ 
 volatile uint32 rcValue[8] = {1500,1500,1500,1500,1500,1500,1500,1500}; // interval [1000;2000]
const int Chan1Pin = 35;         // PC6 Input from RC receiver  
const int Chan2Pin = 36;         // PC7 Input from RC receiver Roll
const int Chan3Pin = 37;         // PC8 Input from RC receiver Throttle
const int Chan4Pin = 38;         // PC9 Input from RC receiver Yaw
const int Chan5Pin = 6;        // PA8 Input from RC receiver Chan5 (gear)
const int Chan6Pin = 7;        // PA9 Input from RC receiver Chan5 (gear)
uint32 RxChStart1,RxChStart2,RxChStart3,RxChStart4;
uint32 RxCh1,RxCh2,RxCh3,RxCh4;
//----------------------------------------------------------------------------
const int  Motor1Pin = 24;      //pb9     4_CH4 
const int  Motor2Pin = 14;      //PB8    4_CH3
const int  Motor3Pin = 9;        //PB7   4_ch2
const int  Motor4Pin = 5;         //PB6  4_ch1
const int  Motor5Pin = 42;        //PB3  目前maple cores不支持，需要修改CORES代码重映射IO才可以 
const int  Motor6Pin = 41;        //PA15目前maple cores不支持，需要修改CORES代码重映射IO才可以 
const int  Motor7Pin = 1;         //PA2   2_ch3
const int  Motor8Pin = 0;        //PA3    2_ch4




